#include<geometry_msgs/Vector3Stamped.h>
#include<geometry_msgs/PoseStamped.h>
#include<geometry_msgs/TwistStamped.h>
#include<mavros_msgs/PositionTarget.h>
#include<mavros_msgs/AttitudeTarget.h>
#include<Eigen/Dense>
#include<tf/tf.h>

//!!!参考pos_controller_PID.h
#define mass 1.5
#define NUM_POINT 2
#define NUM_MOTOR 4

#define MOTOR_P1 -0.00069
#define MOTOR_P2 0.01271
#define MOTOR_P3 -0.07948
#define MOTOR_P4 0.3052
#define MOTOR_P5 0.008775

#define thrust_max_single_motor 6.0


//限幅
double constraint1(double data,double max)//绝对值是否超出
{
    if(abs(data)>max)
    {
        return (data>0)? max:-max;
    }
    else
    {
        return data;
    }
}

double constraint2(double data,double min_,double max)
{
    if(data>max)
    {
        return max;
    }
    else if(data<min_)
    {
        return min_;
    }
    else
    {
        return data;
    }
}

//旋转矩阵转欧拉角
void rotation_to_euler(const Eigen::Matrix3d dcm, Eigen::Vector3d& euler_angle)
{
    double phi_val = atan2(dcm(2, 1), dcm(2, 2));
    double theta_val = asin(-dcm(2, 0));
    double psi_val = atan2(dcm(1, 0), dcm(0, 0));
    double pi = M_PI;

    if (fabs(theta_val - pi / 2.0) < 1.0e-3) {
        phi_val = 0.0;
        psi_val = atan2(dcm(1, 2), dcm(0, 2));

    } else if (fabs(theta_val + pi / 2.0) <  1.0e-3) {
        phi_val = 0.0;
        psi_val = atan2(-dcm(1, 2), -dcm(0, 2));
    }

    euler_angle(0) = phi_val;
    euler_angle(1) = theta_val;
    euler_angle(2) = psi_val;
}

Eigen::Vector3d accel2thrust(Eigen::Vector3d accel)
{
    double tilt_max=20;
    Eigen::Vector3d thrust_sp;

    //除以电机个数得到单个电机的期望推力
    thrust_sp = mass * accel / NUM_MOTOR;

    // 推力限幅，根据最大倾斜角及最大油门
    double thrust_max_XY_tilt = fabs(thrust_sp[2]) * tanf(tilt_max/180.0*M_PI);
    double thrust_max_XY = sqrtf(thrust_max_single_motor * thrust_max_single_motor - pow(thrust_sp[2],2));
    thrust_max_XY = min(thrust_max_XY_tilt, thrust_max_XY);

    if ((pow(thrust_sp[0],2) + pow(thrust_sp[1],2)) > pow(thrust_max_XY,2)) 
    {
        double mag = sqrtf((pow(thrust_sp[0],2) + pow(thrust_sp[1],2)));
        thrust_sp[0] = thrust_sp[0] / mag * thrust_max_XY;
        thrust_sp[1] = thrust_sp[1] / mag * thrust_max_XY;
    }
    
    return thrust_sp;   
}

Eigen::Vector3d thrust2throttle( Eigen::Vector3d thrust_sp)
{
    Eigen::Vector3d throttle_sp;

    //电机模型，可通过辨识得到，推力-油门曲线
    for (int i=0; i<3; i++)
    {
        throttle_sp[i] = MOTOR_P1 * pow(thrust_sp[i],4) + MOTOR_P2 * pow(thrust_sp[i],3) + MOTOR_P3 * pow(thrust_sp[i],2) + MOTOR_P4 * thrust_sp[i] + MOTOR_P5;
        // PX4内部默认假设 0.5油门为悬停推力 ， 在无人机重量为1kg时，直接除20得到0.5
         //throttle_sp[i] = thrust_sp[i]/3;
    }
    return throttle_sp; 
}


void  ThrottleToAttitude(geometry_msgs::Quaternion &desired_att_q,double &desired_throttle,const Eigen::Vector3d thr_sp, double yaw_sp)
{
    Eigen::Vector3d att_sp;
    att_sp[2] = yaw_sp;

    // desired body_z axis = -normalize(thrust_vector)
    Eigen::Vector3d body_x, body_y, body_z;

    double thr_sp_length = thr_sp.norm();

    //cout << "thr_sp_length : "<< thr_sp_length << endl;

    if (thr_sp_length > 0.00001f) {
            body_z = thr_sp.normalized();

    } else {
            // no thrust, set Z axis to safe value
            body_z = Eigen::Vector3d(0.0f, 0.0f, 1.0f);
    }

    // vector of desired yaw direction in XY plane, rotated by PI/2
    Eigen::Vector3d y_C = Eigen::Vector3d(-sinf(yaw_sp),cosf(yaw_sp),0.0);

    if (fabsf(body_z(2)) > 0.000001f) {
            // desired body_x axis, orthogonal to body_z
            body_x = y_C.cross(body_z);

            // keep nose to front while inverted upside down
            if (body_z(2) < 0.0f) {
                    body_x = -body_x;
            }

            body_x.normalize();

    } else {
            // desired thrust is in XY plane, set X downside to construct correct matrix,
            // but yaw component will not be used actually
            body_x = Eigen::Vector3d(0.0f, 0.0f, 0.0f);
            body_x(2) = 1.0f;
    }

    // desired body_y axis
    body_y = body_z.cross(body_x);

    Eigen::Matrix3d R_sp;

    // fill rotation matrix
    for (int i = 0; i < 3; i++) {
            R_sp(i, 0) = body_x(i);
            R_sp(i, 1) = body_y(i);
            R_sp(i, 2) = body_z(i);
    }

    Eigen::Quaterniond q_sp(R_sp);

    rotation_to_euler(R_sp, att_sp);


    //cout << "Desired euler [R P Y]: "<< att_sp[0]* 180/M_PI <<" [deg] " << att_sp[1]* 180/M_PI <<" [deg] "<< att_sp[2]* 180/M_PI <<" [deg] "<< endl;
    //cout << "Desired Thrust: "<< thr_sp_length<< endl;
//    cout << "q_sp [x y z w]: "<< q_sp.x() <<" [ ] " << q_sp.y() <<" [ ] "<<q_sp.z() <<" [ ] "<<q_sp.w() <<" [ ] "<<endl;
//    cout << "R_sp : "<< R_sp(0, 0) <<" " << R_sp(0, 1) <<" "<< R_sp(0, 2) << endl;
//    cout << "     : "<< R_sp(1, 0) <<" " << R_sp(1, 1) <<" "<< R_sp(1, 2) << endl;
//    cout << "     : "<< R_sp(2, 0) <<" " << R_sp(2, 1) <<" "<< R_sp(2, 2) << endl;

    Eigen::Vector3d throttle_sp;

    throttle_sp[0] = thr_sp[0];
    throttle_sp[1] = thr_sp[1];
    throttle_sp[2] = thr_sp[2];

    //期望油门
    //double desired_throttle;//输出需要

    desired_throttle = thr_sp_length; 

    //geometry_msgs::Quaternion desired_att_q;//输出需要

    
    desired_att_q.w = q_sp.w();
    desired_att_q.x = q_sp.x();
    desired_att_q.y = q_sp.y();
    desired_att_q.z = q_sp.z();

    Eigen::Vector3d desired_attitude;
    desired_attitude[0] = att_sp[0];  
    desired_attitude[1] = att_sp[1]; 
    desired_attitude[2] = att_sp[2]; 

}

Eigen::Vector3d accel2throttle(Eigen::Vector3d accel)
{
    Eigen::Vector3d thrust_sp,throttle_sp;
    thrust_sp=accel2thrust(accel);
    throttle_sp=thrust2throttle(thrust_sp);
    return throttle_sp;
}


void  state_cal(Eigen::Vector3d &pos_cal,Eigen::Vector3d &twt_cal,geometry_msgs::PoseStamped last_pos,geometry_msgs::TwistStamped last_twst,Eigen::Vector3d opt_acc,double dt)
{

    Eigen::Vector3d pos;
    pos[0]=last_pos.pose.position.x;
    pos[1]=last_pos.pose.position.y;
    pos[2]=last_pos.pose.position.z;

    Eigen::Vector3d twt;
    twt[0]=last_twst.twist.linear.x;
    twt[1]=last_twst.twist.linear.y;
    twt[2]=last_twst.twist.linear.z;

    //引用加速度优化这一轮的回调数据计算误差

    for(int i=0;i<3;i++)
    {
        pos_cal[i]=pos[i]+twt[i]*dt+0.5*opt_acc[i]*pow(dt,2);
        twt_cal[i]=twt[i]+opt_acc[i]*dt;
    } 
    

}

void state_err(Eigen::Vector3d &err_pos,Eigen::Vector3d &err_vel,Eigen::Vector3d pos_cal,Eigen::Vector3d vel_cal,geometry_msgs::PoseStamped pos,geometry_msgs::TwistStamped twst)
{
    //位置误差
    err_pos[0]=pos_cal[0]-pos.pose.position.x;
    err_pos[1]=pos_cal[1]-pos.pose.position.y;
    err_pos[2]=pos_cal[2]-pos.pose.position.z;

    //限幅，xy方向0.6，z方向1
    for(int i=0;i<2;i++)
    {
        err_pos[i]=constraint1(err_pos[i],0.6);
    }
    err_pos[2]=constraint1(err_pos[2],1);

    //速度误差
    err_vel[0]=vel_cal[0]-twst.twist.linear.x;
    err_vel[1]=vel_cal[1]-twst.twist.linear.y;
    err_vel[2]=vel_cal[2]-twst.twist.linear.z;

    //限幅，xy方向0.3，z方向1
    for(int i=0;i<2;i++)
    {
        err_vel[i]=constraint1(err_vel[i],0.3);
    }
    err_vel[2]=constraint1(err_vel[2],1);

}


///_AttitudeReference分解为orientation+thrust
void att_convert(mavros_msgs::AttitudeTarget &output,geometry_msgs::Quaternion input,double thrust_sp)
{
    output.type_mask=0b00000111;//00 000 111
    output.orientation=input;
    output.thrust=thrust_sp;
}

void casOPT(Eigen::Vector3d &output,Eigen::Vector3d input)
{

    double MPC_THR_MAX=0.9;
    double MPC_THR_MIN=0.1;
    double tilt_max=5;
    double thrust_X,thrust_Y,thrust_Z;
    thrust_Z=input[2]+0.5;
    output[2]=constraint2(thrust_Z,MPC_THR_MIN,MPC_THR_MAX);//油门0.1-0.9
    
    float thrust_max_XY_tilt = fabs(output[2]) * tanf(tilt_max/180.0*M_PI);
    float thrust_max_XY = sqrtf(MPC_THR_MAX * MPC_THR_MAX - output[2] * output[2]);
    thrust_max_XY = min(thrust_max_XY_tilt, thrust_max_XY);
    
    output[0]=thrust_X;
    output[1]=thrust_Y;

    if ((thrust_X * thrust_X + thrust_Y * thrust_Y) > thrust_max_XY * thrust_max_XY) {
            float mag = sqrtf((thrust_X * thrust_X + thrust_Y * thrust_Y));
            output[0] = thrust_X / mag * thrust_max_XY;
            output[1] = thrust_Y / mag * thrust_max_XY;
    }


}